// FS
#include <FS.h>
#include <SPIFFS.h>

// WiFi
#include <IotWebConf.h>
#include <IotWebConfUsing.h>

// Sensors
#include <MS5611_SPI.h>
#include <Wire.h>

// basic
#include <math.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <time.h>

/***** Private TypeDef *****/
typedef struct {
    float xv[4];
    float yv[4];
    float input;
    float output;
} Bw50HzLPFTypeDef;
Bw50HzLPFTypeDef AltitudeLPF_50;

typedef struct FBMTYPE {
    int32_t ADPress;
    int32_t ADTemp;
    float RP;
    float RT;
    float RPFilter;
    float InitAlt;
    float Altitude;
    float AltitudeFilter;
    uint16_t C0, C1, C2, C3, C6, C8, C9, C10, C11, C12;
    uint32_t C4, C5, C7;

} FBMTYPE;
FBMTYPE FBM;

/***** Function Declaration *****/
void READ_5611();
void appendFile(fs::FS& fs, const char* path, const char* message, File file);
String getContentType(String filename);
bool exists(String path);
bool handleFileRead(String path);
float Butterworth50HzLPF(Bw50HzLPFTypeDef* pLPF);

/***** Macro Definition *****/
/* GPIO Declaration */
#define GPIO_KEY 33
#define GPIO_HALL 40
#define GPIO_FIRE 4
#define GPIO_PARACHUTE 7
#define GPIO_SPARE 11
#define GPIO_FIRE_SIGN 5
#define GPIO_PARACHUTE_SIGN 8
#define GPIO_SPARE_SIGN 12
#define GPIO_RUN_SIGN 39

// Sensors GPIO
#define SPI_CLK
#define SPI_MISO
#define SPI_MOSI
#define MS5611_CS 13
#define ICM42688_CS 17
#define I2C_SCL
#define I2C_SDA

/* State Definition */
#define KEY_IS_PRESSED 0
#define HALL_IS_LAUNCHED 1

/* Filter Definition */
#define GAINBtw50hz (3.450423889e+02f)

/* Time Definition */
#define DELTA_T_DETACH 1
#define DELTA_T_PARACHUTE 1

/* Height Definition */
#define HEIGHT_LAND 5

/* FSBroswer Definition */
#define FILESYSTEM SPIFFS
#define FORMAT_FILESYSTEM true
#define DBG_OUTPUT_PORT Serial

// web config
#define CONFIG_VERSION "dem2"
#define CONFIG_PIN GPIO_KEY
#define STRING_LEN 32
#define NUMBER_LEN 32

/* IIC Definition */
#define SCL 26
#define SDA 21
#define SDA_H digitalWrite(SDA, HIGH)
#define SCL_H digitalWrite(SCL, HIGH)
#define SDA_L digitalWrite(SDA, LOW)
#define SCL_L digitalWrite(SCL, LOW)
#define SDA_read digitalRead(SDA)

#define FBMAddr 0xD8
#define SPI_CTRL 0x00
#define FBM_ID 0x6B
#define FBM_COEFF1 0xAA
#define FBM_COEFF2 0xBB
#define FBM_COEFF3 0xD0
#define FBM_COEFF4 0xF1
#define FBM_RESET 0xE0
#define FBM_CONFIG 0xF4
#define DATA_MSB 0xF6
#define DATA_CSB 0xF7
#define DATA_LSB 0xF8
#define OSR1024 0x00
#define OSR2048 0x40
#define OSR4096 0x80
#define OSR8192 0xC0
#define PRES_CONVERSION 0x34
#define TEMP_CONVERSION 0x2E
#define FBMRESET 0xB6
#define FBMID 0x42

// -- Initial name of the Thing. Used e.g. as SSID of the own Access Point.
char FileNameValue[STRING_LEN];
char H_ParaValue[NUMBER_LEN];
char T_DetachValue[NUMBER_LEN];
char T_ParaValue[NUMBER_LEN];
const char thingName[] = "rocket";
// -- Initial password to connect to the Thing, when it creates an own Access Point.
const char wifiInitialApPassword[] = "88888888";
// -- Method declarations.
void handleRoot();
// -- Callback methods.
void configSaved();
bool formValidator(iotwebconf::WebRequestWrapper* webRequestWrapper);
DNSServer dnsServer;
WebServer server(80);
IotWebConf iotWebConf(thingName, &dnsServer, &server, wifiInitialApPassword, CONFIG_VERSION);
IotWebConfParameterGroup group1 = IotWebConfParameterGroup("发射定时参数", "发射定时参数");
IotWebConfParameterGroup group2 = IotWebConfParameterGroup("发射定高参数", "发射定高参数");
IotWebConfParameterGroup group3 = IotWebConfParameterGroup("文件名", "文件名");
IotWebConfTextParameter FileNameParam =
    IotWebConfTextParameter("文件名", "FileName", FileNameValue, STRING_LEN);
IotWebConfNumberParameter T_DetachParam = IotWebConfNumberParameter(
    "二级点火时间", "T_DetachParam", T_DetachValue, NUMBER_LEN, "1", "e.g. 23.4", "step='0.1'");
IotWebConfNumberParameter T_ParaParam = IotWebConfNumberParameter("开伞时间", "T_ParaParam", T_ParaValue,
                                                                  NUMBER_LEN, "6", "e.g. 23.4", "step='0.1'");
IotWebConfNumberParameter H_ParaParam = IotWebConfNumberParameter(
    "开伞高度", "H_ParaParam", H_ParaValue, NUMBER_LEN, "20", "1..100", "min='0' max='100' step='1'");

/***** Priavte Varibles Definition*****/
/* Main Program Varibles */
unsigned long time_launch;
unsigned long time_para;
float current_time;
float height_calced;
float altitude = 0;
float altitude_filter = 0;
float PRES = 0;
float TEMP = 0;
char straltitude[20];
char straltitude_filter[20];
char strtime[20];
char InformationToPrint[100];
float altitude_init = 0;
float altitide_launch = 0;
float T_DETACH = 1.0;
float T_PARACHUTE = 6.0;
int HEIGHT_PARACHUTE = 65;
char FileName[10] = "/0809.txt";

/***** State Machine Definition *****/
enum mission_stage_t { STAND_BY, PRE_LAUNCH, LAUNCHED, DETACHED, PRELAND, LANDED };
mission_stage_t MISSION_STAGE;

/***** Class Definition *****/
MS5611_SPI MS5611(MS5611_CS);  // ESP32 HW SPI

File fsUploadFile;

/***** Interrupt Function Definition *****/
void Hall_Interrupt() {
    if (MISSION_STAGE == PRE_LAUNCH) {
        time_launch = millis();  //单位ms
        digitalWrite(GPIO_RUN_SIGN, HIGH);
        MISSION_STAGE = LAUNCHED;
        Serial.printf("Hall Interrupt Called\r\n");
        // appendFile(SPIFFS, "/a.txt", "Hall Interrupt Called!\r\n", file);
        altitide_launch = altitude_filter;
    }
}

bool handleFileRead(String path) {
    DBG_OUTPUT_PORT.println("handleFileRead: " + path);
    if (path.endsWith("/")) {
        path += "index.htm";
    }
    String contentType = getContentType(path);
    String pathWithGz = path + ".gz";
    if (exists(pathWithGz) || exists(path)) {
        if (exists(pathWithGz)) {
            path += ".gz";
        }
        File file = FILESYSTEM.open(path, "r");
        server.streamFile(file, contentType);
        file.close();
        return true;
    }
    return false;
}

/***** Initialization Fuction Definition *****/
void setup() {
    Serial.begin(115200);
    Serial.println("Program Started!");

    /** init gpio **/
    /* init fire pin */
    pinMode(GPIO_FIRE, OUTPUT);
    pinMode(GPIO_PARACHUTE, OUTPUT);
    pinMode(GPIO_SPARE, OUTPUT);
    pinMode(ICM42688_CS, OUTPUT);
    digitalWrite(ICM42688_CS, HIGH);
    digitalWrite(GPIO_FIRE, LOW);
    digitalWrite(GPIO_PARACHUTE, LOW);
    digitalWrite(GPIO_SPARE, LOW);

    /* init sign pin */
    pinMode(GPIO_FIRE_SIGN, OUTPUT);
    pinMode(GPIO_PARACHUTE_SIGN, OUTPUT);
    pinMode(GPIO_SPARE_SIGN, OUTPUT);
    pinMode(GPIO_RUN_SIGN, OUTPUT);
    digitalWrite(GPIO_FIRE_SIGN, HIGH);
    digitalWrite(GPIO_PARACHUTE_SIGN, HIGH);
    digitalWrite(GPIO_SPARE_SIGN, LOW);
    digitalWrite(GPIO_RUN_SIGN, HIGH);

    /* init KEY & HALL Pin */
    pinMode(GPIO_KEY, INPUT_PULLUP);
    pinMode(GPIO_HALL, INPUT_PULLDOWN);
    attachInterrupt(GPIO_HALL, Hall_Interrupt, RISING);

    /* init IIC pin */
    pinMode(SDA, OUTPUT);
    pinMode(SCL, OUTPUT);
    digitalWrite(SDA, HIGH);
    digitalWrite(SCL, HIGH);

    Serial.println("GPIO initalized!");

    /** init MS5611 **/
    if (MS5611.begin() == true) {
        Serial.println("MS5611 found.");
    } else {
        Serial.println("MS5611 not found. halt.");
        while (1) {
            Serial.println("114514");
        }
    }
    float altitude_sum = 0;
    MS5611.setOversampling(OSR_ULTRA_HIGH);

    delay(7000);
    for (int i = 0; i < 100; i++) {
        READ_5611();
        // Serial.println(altitude);
        altitude_sum += altitude;
    }
    altitude_init = altitude_sum / 100.0;
    Serial.printf("Initial Altitude is:%.4f/r/n", altitude_init);

    /** init FBM320 **/
    FBM320_Init();
    altitude_sum = 0;
    for (int i = 0; i < 100; i++) {
        FBM320_GetAltitude();
        altitude_sum += FBM.Altitude;
    }
    FBM.InitAlt = altitude_sum / 100.0;
    altitude_init = FBM.InitAlt;  // todo
    Serial.printf("Initial Altitude is:%.4f/r/n", FBM.InitAlt);

    /** init FSBrowers **/
    FILESYSTEM.begin(FORMAT_FILESYSTEM);
    Serial.println("FS mounted!");
    group1.addItem(&T_DetachParam);
    group1.addItem(&T_ParaParam);
    group2.addItem(&H_ParaParam);
    group3.addItem(&FileNameParam);

    iotWebConf.setConfigPin(CONFIG_PIN);
    iotWebConf.addParameterGroup(&group1);
    iotWebConf.addParameterGroup(&group2);
    iotWebConf.addParameterGroup(&group3);
    iotWebConf.setConfigSavedCallback(&configSaved);
    iotWebConf.setFormValidator(&formValidator);
    iotWebConf.init();
    T_DETACH = strtof(T_DetachValue, NULL);
    T_PARACHUTE = strtof(T_ParaValue, NULL);
    HEIGHT_PARACHUTE = atoi(H_ParaValue);
    Serial.print("T_DETACH is");
    Serial.println(T_DETACH);
    Serial.print("T_PARACHUTE is");
    Serial.println(T_PARACHUTE);
    Serial.print("HEIGHT_PARACHUTE is");
    Serial.println(HEIGHT_PARACHUTE);
    server.on("/", handleRoot);
    server.on("/config", [] { iotWebConf.handleConfig(); });
    server.onNotFound([]() {
        if (!handleFileRead(server.uri())) {
            iotWebConf.handleNotFound();
        }
    });

    Serial.println("HTTP server started");
    Serial.println("initialize done!");

    /***** Write Current Time *****/
    // time_t t;
    // struct tm* lt;
    // time(&t);            //获取Unix时间戳。
    // lt = localtime(&t);  //转为时间结构。
    // Serial.printf("%d/%d/%d %d:%d:%d\n", lt->tm_year + 1900, lt->tm_mon, lt->tm_mday, lt->tm_hour,
    // lt->tm_min,
    //               lt->tm_sec);  //输出结果

    // TODO:LED STATE
}

/***** Loop Function Definition *****/
void loop() {
    // TODO: auto create new file

    static File file = SPIFFS.open(FileName, FILE_APPEND);

    iotWebConf.doLoop();

    switch (MISSION_STAGE) {
        case STAND_BY:
            if (digitalRead(GPIO_KEY) == KEY_IS_PRESSED) {
                MISSION_STAGE = PRE_LAUNCH;
                digitalWrite(GPIO_RUN_SIGN, LOW);
                Serial.println("Mission start!");
                file.println("Mission start!");
            }
            break;

        case PRE_LAUNCH:
            break;

        case LAUNCHED:
            // record height
            file.printf("%f\t%f\t%f\r\n", current_time, altitude, altitude_filter);
            if ((millis() - time_launch) > T_DETACH * 1000) {
                digitalWrite(GPIO_FIRE, HIGH);
                digitalWrite(GPIO_FIRE_SIGN, LOW);
                MISSION_STAGE = DETACHED;
                Serial.println("Detached!");
                file.println("Detached!");
            }

            break;

        case DETACHED:
            // record height
            file.printf("%f\t%f\t%f\r\n", current_time, altitude, altitude_filter);
            if ((millis() - time_launch) > (T_DETACH + DELTA_T_DETACH) * 1000) {
                digitalWrite(GPIO_FIRE, LOW);
            }
            if (altitude_filter > HEIGHT_PARACHUTE) {
                time_para = millis();
                digitalWrite(GPIO_PARACHUTE, HIGH);
                digitalWrite(GPIO_PARACHUTE_SIGN, LOW);
                MISSION_STAGE = PRELAND;
                Serial.printf("Height Parachute on!\r\n");
                file.println("Height Parachute on!");
            }
            if ((millis() - time_launch) > T_PARACHUTE * 1000) {
                time_para = millis();
                digitalWrite(GPIO_PARACHUTE, HIGH);
                digitalWrite(GPIO_PARACHUTE_SIGN, LOW);
                MISSION_STAGE = PRELAND;
                Serial.printf("Time Parachute on!\r\n");
                file.println("Time Parachute on!");
            }

            break;

        case PRELAND:
            // record height
            file.printf("%f\t%f\t%f\r\n", current_time, altitude, altitude_filter);
            if ((millis() - time_para) > DELTA_T_PARACHUTE * 1000) {
                digitalWrite(GPIO_PARACHUTE, LOW);
            }
            if (altitude_filter < HEIGHT_LAND) {
                MISSION_STAGE = LANDED;
                Serial.printf("Preland!\r\n");
                file.println("Preland!");
            }
            break;

        case LANDED:
            // stop record
            file.println("Landed!");
            file.close();
            // SPIFFS.remove(FileName);
            digitalWrite(GPIO_FIRE_SIGN, HIGH);
            digitalWrite(GPIO_PARACHUTE_SIGN, HIGH);
            digitalWrite(GPIO_SPARE_SIGN, LOW);
            digitalWrite(GPIO_RUN_SIGN, HIGH);
            MISSION_STAGE = STAND_BY;
            break;

        default:
            break;
    }
    // READ_5611();
    FBM320_GetAltitude();
    altitude = FBM.Altitude;
    AltitudeLPF_50.input = altitude;
    altitude_filter = Butterworth50HzLPF(&AltitudeLPF_50);
    current_time = (millis() - time_launch) / 1000.0;
    altitude -= altitude_init;
    altitude -= altitide_launch;
    altitude_filter -= altitude_init;
    altitude_filter -= altitide_launch;
    Serial.printf("%.4f\t %.4f\t\r\n", altitude, altitude_filter);
    server.handleClient();
}

void READ_5611() {
    int result = MS5611.read();
    if (result != MS5611_READ_OK) {
        Serial.print("Error in read: ");
        Serial.println(result);
    } else {
        TEMP = MS5611.getTemperature() + 273.15;
        PRES = MS5611.getPressure() * 0.1;
        // altitude = (pow(((double)(101.325)) / ((double)(PRES)), 1.0 / 5.257) - 1) * TEMP / 0.0065;
        altitude = 44330 * (1 - pow(((double)(PRES)) / ((double)(101.325)), 1.0 / 5.255));
        AltitudeLPF_50.input = altitude;
        altitude_filter = Butterworth50HzLPF(&AltitudeLPF_50);
    }
}

// void appendFile(fs::FS& fs, const char* path, const char* message, File file) {
//     Serial.printf("Appending to file: %s\r\n", path);
//     if (!file) {
//         Serial.println("- failed to open file for appending");
//         return;
//     }
//     if (file.print(message)) {
//         Serial.println("- message appended");
//     } else {
//         Serial.println("- append failed");
//     }
// }

String getContentType(String filename) {
    if (server.hasArg("download")) {
        return "application/octet-stream";
    } else if (filename.endsWith(".htm")) {
        return "text/html";
    } else if (filename.endsWith(".html")) {
        return "text/html";
    } else if (filename.endsWith(".css")) {
        return "text/css";
    } else if (filename.endsWith(".js")) {
        return "application/javascript";
    } else if (filename.endsWith(".png")) {
        return "image/png";
    } else if (filename.endsWith(".gif")) {
        return "image/gif";
    } else if (filename.endsWith(".jpg")) {
        return "image/jpeg";
    } else if (filename.endsWith(".ico")) {
        return "image/x-icon";
    } else if (filename.endsWith(".xml")) {
        return "text/xml";
    } else if (filename.endsWith(".pdf")) {
        return "application/x-pdf";
    } else if (filename.endsWith(".zip")) {
        return "application/x-zip";
    } else if (filename.endsWith(".gz")) {
        return "application/x-gzip";
    }
    return "text/plain";
}

bool exists(String path) {
    bool yes = false;
    File file = FILESYSTEM.open(path, "r");
    if (!file.isDirectory()) {
        yes = true;
    }
    file.close();
    return yes;
}

void handleRoot() {
    // -- Let IotWebConf test and handle captive portal requests.
    if (iotWebConf.handleCaptivePortal()) {
        // -- Captive portal request were already served.
        return;
    }
    String s =
        "<!DOCTYPE html><html lang=\"en\"><head><meta name=\"viewport\" content=\"width=device-width, "
        "initial-scale=1, user-scalable=no\"/>";
    s += "<title>IotWebConf 03 Custom Parameters</title></head><body>Hello world!";

    s += "Go to <a href='config'>configure page</a> to change values.";
    s += "</body></html>\n";

    server.send(200, "text/html", s);
}

void configSaved() {
    Serial.println("Configuration was updated.");
    Serial.printf("Detach time is changed to:");
    Serial.println(T_DetachValue);
    Serial.printf("Parachute time is changed to:");
    Serial.println(T_ParaValue);
    Serial.printf("Parachute height is changed to:");
    Serial.println(H_ParaValue);
    Serial.printf("File name is changed to:");
    Serial.println(FileNameValue);
    T_DETACH = strtof(T_DetachValue, NULL);
    T_PARACHUTE = strtof(T_ParaValue, NULL);
    HEIGHT_PARACHUTE = atoi(H_ParaValue);
}

bool formValidator(iotwebconf::WebRequestWrapper* webRequestWrapper) {
    Serial.println("Validating form.");
    bool valid = true;

    /*
      int l = webRequestWrapper->arg(stringParam.getId()).length();
      if (l < 3)
      {
        stringParam.errorMessage = "Please provide at least 3 characters for this test!";
        valid = false;
      }
    */
    return valid;
}

float Butterworth50HzLPF(Bw50HzLPFTypeDef* pLPF) {
    pLPF->xv[0] = pLPF->xv[1];
    pLPF->xv[1] = pLPF->xv[2];
    pLPF->xv[2] = pLPF->xv[3];
    pLPF->xv[3] = pLPF->input / GAINBtw50hz;
    pLPF->yv[0] = pLPF->yv[1];
    pLPF->yv[1] = pLPF->yv[2];
    pLPF->yv[2] = pLPF->yv[3];
    pLPF->yv[3] = (pLPF->xv[0] + pLPF->xv[3]) + 3 * (pLPF->xv[1] + pLPF->xv[2]) +
                  (0.5320753683f * pLPF->yv[0]) + (-1.9293556691f * pLPF->yv[1]) +
                  (2.3740947437f * pLPF->yv[2]);

    pLPF->output = pLPF->yv[3];
    return pLPF->output;
}

void SDA_OUT(void) { pinMode(SDA, OUTPUT); }

void SDA_IN(void) { pinMode(SDA, INPUT); }

void Delay_us(int n) { delayMicroseconds(n); }

void IIC_Start(void) {
    SDA_OUT();
    SDA_H;
    SCL_H;
    Delay_us(4);
    SDA_L;
    Delay_us(4);
    SCL_L;
}

void IIC_Stop(void) {
    SDA_OUT();
    SCL_L;
    SDA_L;
    Delay_us(4);
    SCL_H;
    SDA_H;
    Delay_us(4);
}

uint8_t IIC_WaitAck(void) {
    uint8_t ucErrTime = 0;
    SDA_IN();  // SDAÉèÖÃÎªÊäÈë  £¨´Ó»ú¸øÒ»¸öµÍµçÆ½×öÎªÓ¦´ð£©
    SDA_H;
    Delay_us(1);
    SCL_H;
    Delay_us(1);
    while (SDA_read) {
        ucErrTime++;
        if (ucErrTime > 250) {
            IIC_Stop();
            return 1;
        }
    }
    SCL_L;  //Ê±ÖÓÊä³öµÍ
    return 0;
}

void IIC_Ack(void) {
    SCL_L;
    SDA_OUT();
    SDA_L;
    Delay_us(1);
    SCL_H;
    Delay_us(2);
    SCL_L;
}

void IIC_NAck(void) {
    SCL_L;
    SDA_OUT();
    SDA_H;
    Delay_us(1);
    SCL_H;
    Delay_us(1);
    SCL_L;
}

void IIC_SendByte(uint8_t data) {
    uint8_t t;
    SDA_OUT();
    SCL_L;  //À­µÍÊ±ÖÓ¿ªÊ¼Êý¾Ý´«Êä
    for (t = 0; t < 8; t++) {
        if (data & 0x80)
            SDA_H;
        else
            SDA_L;
        data <<= 1;
        Delay_us(1);
        SCL_H;
        Delay_us(1);
        SCL_L;
        Delay_us(1);
    }
}

uint8_t IIC_ReadByte(uint8_t ack) {
    uint8_t i, receive = 0;
    SDA_IN();  // SDAÉèÖÃÎªÊäÈëÄ£Ê½ µÈ´ý½ÓÊÕ´Ó»ú·µ»ØÊý¾Ý
    for (i = 0; i < 8; i++) {
        SCL_L;
        Delay_us(1);
        SCL_H;
        receive <<= 1;
        if (SDA_read) receive++;  //´Ó»ú·¢ËÍµÄµçÆ½
        Delay_us(1);
    }
    if (ack)
        IIC_Ack();  //·¢ËÍACK
    else
        IIC_NAck();  //·¢ËÍnACK
    return receive;
}

uint8_t IIC_ReadByteFromSlave(uint8_t I2C_Addr, uint8_t reg, uint8_t* buf) {
    IIC_Start();
    IIC_SendByte(I2C_Addr);  //·¢ËÍ´Ó»úµØÖ·
    if (IIC_WaitAck())       //Èç¹û´Ó»úÎ´Ó¦´ðÔòÊý¾Ý·¢ËÍÊ§°Ü
    {
        IIC_Stop();
        return 1;
    }
    IIC_SendByte(reg);  //·¢ËÍ¼Ä´æÆ÷µØÖ·
    IIC_WaitAck();

    IIC_Start();
    IIC_SendByte(I2C_Addr + 1);  //½øÈë½ÓÊÕÄ£Ê½
    IIC_WaitAck();
    *buf = IIC_ReadByte(0);
    IIC_Stop();  //²úÉúÒ»¸öÍ£Ö¹Ìõ¼þ
    return 0;
}

uint8_t IIC_WriteByteToSlave(uint8_t I2C_Addr, uint8_t reg, uint8_t data) {
    IIC_Start();
    IIC_SendByte(I2C_Addr);  //·¢ËÍ´Ó»úµØÖ·
    if (IIC_WaitAck()) {
        IIC_Stop();
        return 1;  //´Ó»úµØÖ·Ð´ÈëÊ§°Ü
    }
    IIC_SendByte(reg);  //·¢ËÍ¼Ä´æÆ÷µØÖ·
    IIC_WaitAck();
    IIC_SendByte(data);
    if (IIC_WaitAck()) {
        IIC_Stop();
        return 1;  //Êý¾ÝÐ´ÈëÊ§°Ü
    }
    IIC_Stop();  //²úÉúÒ»¸öÍ£Ö¹Ìõ¼þ
    return 0;
}

uint8_t IIC_ReadMultByteFromSlave(uint8_t dev, uint8_t reg, uint8_t length, uint8_t* data) {
    uint8_t count = 0;
    uint8_t temp;
    IIC_Start();
    IIC_SendByte(dev);  //·¢ËÍ´Ó»úµØÖ·
    if (IIC_WaitAck()) {
        IIC_Stop();
        return 1;  //´Ó»úµØÖ·Ð´ÈëÊ§°Ü
    }
    IIC_SendByte(reg);  //·¢ËÍ¼Ä´æÆ÷µØÖ·
    IIC_WaitAck();
    IIC_Start();
    IIC_SendByte(dev + 1);  //½øÈë½ÓÊÕÄ£Ê½
    IIC_WaitAck();
    for (count = 0; count < length; count++) {
        if (count != (length - 1))
            temp = IIC_ReadByte(1);  //´øACKµÄ¶ÁÊý¾Ý
        else
            temp = IIC_ReadByte(0);  //×îºóÒ»¸ö×Ö½ÚNACK

        data[count] = temp;
    }
    IIC_Stop();  //²úÉúÒ»¸öÍ£Ö¹Ìõ¼þ
    return 0;
}

uint8_t IIC_WriteMultByteToSlave(uint8_t dev, uint8_t reg, uint8_t length, uint8_t* data) {
    uint8_t count = 0;
    IIC_Start();
    IIC_SendByte(dev);  //·¢ËÍ´Ó»úµØÖ·
    if (IIC_WaitAck()) {
        IIC_Stop();
        return 1;  //´Ó»úµØÖ·Ð´ÈëÊ§°Ü
    }
    IIC_SendByte(reg);  //·¢ËÍ¼Ä´æÆ÷µØÖ·
    IIC_WaitAck();
    for (count = 0; count < length; count++) {
        IIC_SendByte(data[count]);
        if (IIC_WaitAck())  //Ã¿Ò»¸ö×Ö½Ú¶¼ÒªµÈ´Ó»úÓ¦´ð
        {
            IIC_Stop();
            return 1;  //Êý¾ÝÐ´ÈëÊ§°Ü
        }
    }
    IIC_Stop();  //²úÉúÒ»¸öÍ£Ö¹Ìõ¼þ
    return 0;
}

uint8_t FBM320_WriteByte(uint8_t reg, uint8_t data) {
    if (IIC_WriteByteToSlave(FBMAddr, reg, data))
        return 1;
    else
        return 0;
}

uint8_t FBM320_ReadByte(uint8_t reg, uint8_t* buf) {
    if (IIC_ReadByteFromSlave(FBMAddr, reg, buf))
        return 1;
    else
        return 0;
}

uint8_t FBM320_WriteMultBytes(uint8_t reg, uint8_t len, uint8_t* buf) {
    if (IIC_WriteMultByteToSlave(FBMAddr, reg, len, buf))
        return 1;
    else
        return 0;
}

uint8_t FBM320_ReadMultBytes(uint8_t reg, uint8_t len, uint8_t* buf) {
    if (IIC_ReadMultByteFromSlave(FBMAddr, reg, len, buf))
        return 1;
    else
        return 0;
}

uint8_t FBM320_getDeviceID(void) {
    uint8_t buf;
    FBM320_ReadByte(FBM_ID, &buf);
    return buf;
}

uint8_t FBM320_testConnection(void) {
    if (FBM320_getDeviceID() == FBMID)  // 0x42
        return 1;
    else
        return 0;
}

void FBM320_Check(void) {
    while (!FBM320_testConnection()) {
        Serial.printf("\rFBM320 no connect...\r\n");
    }
    Serial.printf("Connected\r\n");
}

void FBM320_Init(void) {
    FBM320_Check();  //¼ì²éFBM320ÓëMCUÊÇ·ñÕý³£Í¨ÐÅ

    FBM320_WriteByte(FBM_RESET, FBMRESET);  //¸´Î»FBM320
    delay(100);
    FBM320_GetCoeff();
}

/******************************************************************************
 * 函  数：void FBM320_GetAltitude(void)
 * 功  能：FBM320的气压值转换成相对高度
 * 参  数：无
 * 返回值：无
 * 备  注：注意：此函数调用频率最好200Hz,否则获取的高度值误差较大！！！
 *******************************************************************************/
void FBM320_GetAltitude(void)  // 200Hz
{
    uint8_t buf[3];
    FBM320_WriteByte(FBM_CONFIG, TEMP_CONVERSION);  // 280us
    delay(5);

    FBM320_ReadMultBytes(DATA_MSB, 3, buf);
    FBM.ADTemp = (buf[0] << 16) | (buf[1] << 8) | buf[2];
    delay(1);
    FBM320_WriteByte(FBM_CONFIG, PRES_CONVERSION + OSR8192);  //开启气压转化
    delay(10);

    FBM320_ReadMultBytes(DATA_MSB, 3, buf);
    FBM.ADPress = (buf[0] << 16) | (buf[1] << 8) | buf[2];
    FBM320_WriteByte(FBM_CONFIG, TEMP_CONVERSION);  //开启温度转换
    delay(3);
    FBM320_Calculate(FBM.ADPress,
                     FBM.ADTemp);  //将FBM320的原始数据转换成物理量 气压单位是帕，温度单位摄氏度
    FBM.Altitude = 44330 * (1 - pow(((double)(FBM.RP / 1000.0)) / ((double)(101.325)), 1.0 / 5.255));
    // Serial.printf("Altitude:%0.2f \r\n", FBM.Altitude);
}

void FBM320_GetCoeff(void) {
    uint8_t data[20], i = 0;
    uint16_t R[10];
    FBM320_ReadMultBytes(FBM_COEFF1, 18, data);
    FBM320_ReadByte(FBM_COEFF3, &data[18]);
    FBM320_ReadByte(FBM_COEFF4, &data[19]);
    for (i = 0; i < 10; i++) {
        R[i] = (uint16_t)(data[2 * i] << 8) | data[2 * i + 1];
    }
    /* Use R0~R9 calculate C0~C12 of FBM320-02  */
    FBM.C0 = R[0] >> 4;
    FBM.C1 = ((R[1] & 0xFF00) >> 5) | (R[2] & 7);
    FBM.C2 = ((R[1] & 0xFF) << 1) | (R[4] & 1);
    FBM.C3 = R[2] >> 3;
    FBM.C4 = ((uint32_t)R[3] << 2) | (R[0] & 3);
    FBM.C5 = R[4] >> 1;
    FBM.C6 = R[5] >> 3;
    FBM.C7 = ((uint32_t)R[6] << 3) | (R[5] & 7);
    FBM.C8 = R[7] >> 3;
    FBM.C9 = R[8] >> 2;
    FBM.C10 = ((R[9] & 0xFF00) >> 6) | (R[8] & 3);
    FBM.C11 = R[9] & 0xFF;
    FBM.C12 = ((R[0] & 0x0C) << 1) | (R[7] & 7);

    for (i = 0; i < 10; i++) Serial.printf("R = %d\n", R[i]);
    Serial.printf("C0=%d C1=%d C2=%d\n", FBM.C0, FBM.C1, FBM.C2);
    Serial.printf("C3=%d C4=%d C5=%d\n", FBM.C3, FBM.C4, FBM.C5);
    Serial.printf("C6=%d C7=%d C8=%d\n", FBM.C6, FBM.C7, FBM.C8);
    Serial.printf("C9=%d C10=%d C11=%d\n", FBM.C9, FBM.C10, FBM.C11);
    Serial.printf("C12=%d\n", FBM.C12);
}

void FBM320_Calculate(int32_t UP, int32_t UT) {
    int32_t DT, DT2, X01, X02, X03, X11, X12, X13, X21, X22, X23, X24, X25, X26, X31, X32, CF, PP1, PP2, PP3,
        PP4;

    DT = ((UT - 8388608) >> 4) + (FBM.C0 << 4);
    X01 = (FBM.C1 + 4459) * DT >> 1;
    X02 = ((((FBM.C2 - 256) * DT) >> 14) * DT) >> 4;
    X03 = (((((FBM.C3 * DT) >> 18) * DT) >> 18) * DT);
    FBM.RT = ((2500 << 15) - X01 - X02 - X03) >> 15;

    DT2 = (X01 + X02 + X03) >> 12;

    X11 = ((FBM.C5 - 4443) * DT2);
    X12 = (((FBM.C6 * DT2) >> 16) * DT2) >> 2;
    X13 = ((X11 + X12) >> 10) + ((FBM.C4 + 120586) << 4);

    X21 = ((FBM.C8 + 7180) * DT2) >> 10;
    X22 = (((FBM.C9 * DT2) >> 17) * DT2) >> 12;
    X23 = (X22 >= X21) ? (X22 - X21) : (X21 - X22);

    X24 = (X23 >> 11) * (FBM.C7 + 166426);
    X25 = ((X23 & 0x7FF) * (FBM.C7 + 166426)) >> 11;
    X26 =
        (X21 >= X22) ? (((0 - X24 - X25) >> 11) + FBM.C7 + 166426) : (((X24 + X25) >> 11) + FBM.C7 + 166426);

    PP1 = ((UP - 8388608) - X13) >> 3;
    PP2 = (X26 >> 11) * PP1;
    PP3 = ((X26 & 0x7FF) * PP1) >> 11;
    PP4 = (PP2 + PP3) >> 10;

    CF = (2097152 + FBM.C12 * DT2) >> 3;
    X31 = (((CF * FBM.C10) >> 17) * PP4) >> 2;
    X32 = (((((CF * FBM.C11) >> 15) * PP4) >> 18) * PP4);
    FBM.RP = ((X31 + X32) >> 15) + PP4 + 99880;  // 99880ÆøÑ¹²¹³¥
}
